#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <omp.h>
//#include <semaphore.h>
#include "md5.h"

const uint32_t T[64] = {
	0xd76aa478,	0xe8c7b756,	0x242070db,	0xc1bdceee,	0xf57c0faf,	0x4787c62a,	0xa8304613,	0xfd469501,
	0x698098d8,	0x8b44f7af,	0xffff5bb1,	0x895cd7be,	0x6b901122,	0xfd987193,	0xa679438e,	0x49b40821,
	0xf61e2562,	0xc040b340,	0x265e5a51,	0xe9b6c7aa,	0xd62f105d,	0x2441453,	0xd8a1e681,	0xe7d3fbc8,
	0x21e1cde6,	0xc33707d6,	0xf4d50d87,	0x455a14ed,	0xa9e3e905,	0xfcefa3f8,	0x676f02d9,	0x8d2a4c8a,
	0xfffa3942,	0x8771f681,	0x6d9d6122,	0xfde5380c,	0xa4beea44,	0x4bdecfa9,	0xf6bb4b60,	0xbebfbc70,
	0x289b7ec6,	0xeaa127fa,	0xd4ef3085,	0x4881d05,	0xd9d4d039,	0xe6db99e5,	0x1fa27cf8,	0xc4ac5665,
	0xf4292244,	0x432aff97,	0xab9423a7,	0xfc93a039,	0x655b59c3,	0x8f0ccc92,	0xffeff47d,	0x85845dd1,
	0x6fa87e4f,	0xfe2ce6e0,	0xa3014314,	0x4e0811a1,	0xf7537e82,	0xbd3af235,	0x2ad7d2bb,	0xeb86d391
};

inline uint32_t swap_bytes(uint32_t in)
{
	return (in << 24) | ((in & 0xFF00) << 8) | ((in & 0xFF0000) >> 8) | (in >> 24);
}

uint32_t* pad_bits(uint8_t *message, uint64_t *length)
{
	uint8_t *new_message;
	uint32_t *out;
	uint64_t orig_length;
	int bytes_to_pad;
	int i;
	
	orig_length = *length;
	bytes_to_pad = (448 - ((int)orig_length << 3) % 512) >> 3;
	
	new_message = realloc(message, orig_length + bytes_to_pad + 8);
	out = calloc((orig_length+bytes_to_pad+8)/4,sizeof(uint32_t));

	new_message[orig_length] = 0x80;
	
	#pragma omp parallel for shared(out) firstprivate(new_message,orig_length)
	for(i = 0; i < orig_length+1; i+=4)
	{
		out[i/4] = new_message[i] | (new_message[i+1] << 8) | (new_message[i+2] << 16) | (new_message[i+3] << 24);
	}
	
	*length += bytes_to_pad + 8;
	*length /= 4;
	
	out[*length - 2] = (orig_length*8) & 0xFFFFFFFF;
	out[*length - 1] = ((orig_length*8) & 0xFFFFFFFF00000000) >> 32;
		
	return out;
}

inline uint32_t rotate_left(uint32_t in, int n)
{
	return (in << n) | (in >> (32 - n));
}

inline uint32_t F(uint32_t X, uint32_t Y, uint32_t Z)
{
	return (X & Y) | ((~X) & Z);
}

inline uint32_t G(uint32_t X, uint32_t Y, uint32_t Z)
{
	return (X & Z) | (Y & (~Z));
}

inline uint32_t H(uint32_t X, uint32_t Y, uint32_t Z)
{
	return X ^ Y ^ Z;
}

inline uint32_t I(uint32_t X, uint32_t Y, uint32_t Z)
{
	return Y ^ (X | (~Z));
}

inline uint32_t round_1(uint32_t a, uint32_t b, uint32_t c, uint32_t d, uint32_t X_k, uint32_t s, uint32_t T_i)
{
	return b + rotate_left((a + F(b,c,d) + X_k + T_i), s);
}

inline uint32_t round_2(uint32_t a, uint32_t b, uint32_t c, uint32_t d, uint32_t X_k, uint32_t s, uint32_t T_i)
{
	return b + rotate_left((a + G(b,c,d) + X_k + T_i), s);
}

inline uint32_t round_3(uint32_t a, uint32_t b, uint32_t c, uint32_t d, uint32_t X_k, uint32_t s, uint32_t T_i)
{
	return b + rotate_left((a + H(b,c,d) + X_k + T_i), s);
}

inline uint32_t round_4(uint32_t a, uint32_t b, uint32_t c, uint32_t d, uint32_t X_k, uint32_t s, uint32_t T_i)
{
	return b + rotate_left((a + I(b,c,d) + X_k + T_i), s);
}

void md5_encrypt(uint32_t* message, uint64_t length, uint32_t *output)
{
	int i;
	uint32_t X[16];
	uint32_t AA, BB, CC, DD;
	uint32_t A = 0x67452301;
	uint32_t B = 0xefcdab89;
	uint32_t C = 0x98badcfe;
	uint32_t D = 0x10325476;

	for(i = 0; i < length; i+=16)
	{
		/*A <= D;
		B <= ((A + F(B,C,D) + X[i] + K[i]) <<< s) + B;
		C <= B;
		D <= C;*/
		/*for(j = 0; j < 16; j++)
		{
			X[j] = message[i+j];
		}*/
		memcpy(X,message+i,16*sizeof(uint32_t));
		AA = A;
		BB = B;
		CC = C;
		DD = D;
		
		//printf("Before:\nA = %.8x\nB = %.8x\nC = %.8x\nD = %.8x\nF(B,C,D) = %.8x\n",A,B,C,D,F(B,C,D));
		/*for(j = 0; j < 64; j++)
		{
			if(j >=0 && j < 16)
			{
				F_ptr = &F;
				k = j;
			}
			else if(j >= 16 && j < 32)
			{
				F_ptr = &G;
				k = (5*j + 1) % 16;
			}
			else if(j >=32 && j < 48)
			{
				F_ptr = &H;
				k = (3*j + 5)%16;
			}
			else
			{
				F_ptr = &I;
				k = (7*j)%16;
			}
			#pragma omp parallel shared(myA,myB,myC,myD,F_ptr,j,k,X,s) firstprivate(A,B,C,D)
			{
				#pragma omp sections
				{
					#pragma omp section
					{
						myA = D;
					}
					#pragma omp section
					{
						//printf("F_ptr(B,C,D) = %.8x\n",(*F_ptr)(B,C,D));
						myB = rotate_left(A + (*F_ptr)(B,C,D) + X[k] + T[j],s[j]) + B;
					}
					#pragma omp section
					{
						myC = B;
					}
					#pragma omp section
					{
						myD = C;
					}
				}
			}
			A = myA;
			B = myB;
			C = myC;
			D = myD;
			//printf("After:\nA = %.8x\nB = %.8x\nC = %.8x\nD = %.8x\n",A,B,C,D);
		}*/
		//Round 1
		A = round_1(A,B,C,D,X[0],7,T[0]);
		D = round_1(D,A,B,C,X[1],12,T[1]);
		C = round_1(C,D,A,B,X[2],17,T[2]);
		B = round_1(B,C,D,A,X[3],22,T[3]);
		
		A = round_1(A,B,C,D,X[4],7,T[4]);
		D = round_1(D,A,B,C,X[5],12,T[5]);
		C = round_1(C,D,A,B,X[6],17,T[6]);
		B = round_1(B,C,D,A,X[7],22,T[7]);
		
		A = round_1(A,B,C,D,X[8],7,T[8]);
		D = round_1(D,A,B,C,X[9],12,T[9]);
		C = round_1(C,D,A,B,X[10],17,T[10]);
		B = round_1(B,C,D,A,X[11],22,T[11]);
		
		A = round_1(A,B,C,D,X[12],7,T[12]);
		D = round_1(D,A,B,C,X[13],12,T[13]);
		C = round_1(C,D,A,B,X[14],17,T[14]);
		B = round_1(B,C,D,A,X[15],22,T[15]);
		//printf("Round 1:\nA = %.8x\nB = %.8x\nC = %.8x\nD = %.8x\n",A,B,C,D);
		//Round 2
		A = round_2(A,B,C,D,X[1],5,T[16]);
		D = round_2(D,A,B,C,X[6],9,T[17]);
		C = round_2(C,D,A,B,X[11],14,T[18]);
		B = round_2(B,C,D,A,X[0],20,T[19]);
		
		A = round_2(A,B,C,D,X[5],5,T[20]);
		D = round_2(D,A,B,C,X[10],9,T[21]);
		C = round_2(C,D,A,B,X[15],14,T[22]);
		B = round_2(B,C,D,A,X[4],20,T[23]);
		
		A = round_2(A,B,C,D,X[9],5,T[24]);
		D = round_2(D,A,B,C,X[14],9,T[25]);
		C = round_2(C,D,A,B,X[3],14,T[26]);
		B = round_2(B,C,D,A,X[8],20,T[27]);
		
		A = round_2(A,B,C,D,X[13],5,T[28]);
		D = round_2(D,A,B,C,X[2],9,T[29]);
		C = round_2(C,D,A,B,X[7],14,T[30]);
		B = round_2(B,C,D,A,X[12],20,T[31]);
		//printf("Round 2:\nA = %.8x\nB = %.8x\nC = %.8x\nD = %.8x\n",A,B,C,D);
		//Round 3
		A = round_3(A,B,C,D,X[5],4,T[32]);
		D = round_3(D,A,B,C,X[8],11,T[33]);
		C = round_3(C,D,A,B,X[11],16,T[34]);
		B = round_3(B,C,D,A,X[14],23,T[35]);
		
		A = round_3(A,B,C,D,X[1],4,T[36]);
		D = round_3(D,A,B,C,X[4],11,T[37]);
		C = round_3(C,D,A,B,X[7],16,T[38]);
		B = round_3(B,C,D,A,X[10],23,T[39]);
		
		A = round_3(A,B,C,D,X[13],4,T[40]);
		D = round_3(D,A,B,C,X[0],11,T[41]);
		C = round_3(C,D,A,B,X[3],16,T[42]);
		B = round_3(B,C,D,A,X[6],23,T[43]);
		
		A = round_3(A,B,C,D,X[9],4,T[44]);
		D = round_3(D,A,B,C,X[12],11,T[45]);
		C = round_3(C,D,A,B,X[15],16,T[46]);
		B = round_3(B,C,D,A,X[2],23,T[47]);
		//printf("Round 3:\nA = %.8x\nB = %.8x\nC = %.8x\nD = %.8x\n",A,B,C,D);
		//Round 4
		A = round_4(A,B,C,D,X[0],6,T[48]);
		D = round_4(D,A,B,C,X[7],10,T[49]);
		C = round_4(C,D,A,B,X[14],15,T[50]);
		B = round_4(B,C,D,A,X[5],21,T[51]);
		
		A = round_4(A,B,C,D,X[12],6,T[52]);
		D = round_4(D,A,B,C,X[3],10,T[53]);
		C = round_4(C,D,A,B,X[10],15,T[54]);
		B = round_4(B,C,D,A,X[1],21,T[55]);
		
		A = round_4(A,B,C,D,X[8],6,T[56]);
		D = round_4(D,A,B,C,X[15],10,T[57]);
		C = round_4(C,D,A,B,X[6],15,T[58]);
		B = round_4(B,C,D,A,X[13],21,T[59]);
		
		A = round_4(A,B,C,D,X[4],6,T[60]);
		D = round_4(D,A,B,C,X[11],10,T[61]);
		C = round_4(C,D,A,B,X[2],15,T[62]);
		B = round_4(B,C,D,A,X[9],21,T[63]);
		A += AA;
		B += BB;
		C += CC;
		D += DD;
	}
	
	output[0] = swap_bytes(A);
	output[1] = swap_bytes(B);
	output[2] = swap_bytes(C);
	output[3] = swap_bytes(D);
}
